/*
  BenderTestSuite
  Test suite to test functionality of Bender electronics
*/

#include <Max3421e.h>
#include <Usb.h>
#include <AndroidAccessory.h>

#include <Bender.h>

void setup() {
    // open the serial port:
  Serial.begin(115200);
  initBender();

  // put your setup code here, to run once:
}

#define STATE_INIT      0
#define STATE_TURN      1
#define STATE_PAUSE     2
#define STATE_TURN_PAST 3

void loop() {
  static int ledBrightness[8] = {0, 0, 255, 255, 255, 255, 255, 255};
  static int startLed = 0;
  static int renderLed = 0;
  static int state = STATE_INIT;
  static int encountered;
  static unsigned long encounteredAt;
  static unsigned long animTick = millis();


  unsigned long tickNow = millis();
  if (animTick + 100 < tickNow) {
    animTick = tickNow;
    ledBrightness[startLed] = 255;
    startLed = ( startLed + 1 ) % 8;
    ledBrightness[(startLed+1)%8] = 0;
  }
  turnOnLED(renderLed, ledBrightness[renderLed]);
  renderLed = (renderLed + 1) % 8;

  char input = Serial.read();
  if (input == ' ') {
    if (state == STATE_INIT) {
      MotorCtrl(lastDirection, 125);
      state = STATE_TURN;
      encountered = 0;
      Serial.println("Starting...");
    } else {
      MotorCtrl(lastDirection, 0);
      state = STATE_INIT;
    }
  }

  switch (state) {
    case STATE_INIT:
      break;
    case STATE_TURN:
      if (digitalRead(HALL_GS) == LOW) {
        MotorCtrl(lastDirection, 0);
        //Serial.println("Stopping on some hall sensor...");
        encounteredAt = millis();
        if(++encountered==8) {
          //Serial.println("Switching directions.");
          lastDirection = lastDirection == CW ? CCW : CW;
          encountered = 0;
        }
        state = STATE_PAUSE;
      }
      break;
    case STATE_PAUSE:
      if (encounteredAt + 2000 < millis()) {
        //Serial.println("Starting to turn again...");
        MotorCtrl(lastDirection, 125);
        state = STATE_TURN_PAST;
      }
      break;
    case STATE_TURN_PAST:
      if (digitalRead(HALL_GS) == HIGH) {
        //Serial.println("No longer triggering Hall");
        delay(10);
        state = STATE_TURN;
      }
      break;
  }
}
